• 专利附录
  • 专利说明
  • 权利要求
  • 类似技术
  • 同族专利
  • 引用文献
  • 法律状态
  • 优先权
    • 专利摘要:
    There is proposed an alignment method based on a simplified mode allowing a treatment by an invariant Kalman filter, in which each speed involved in the navigation equations is expressed in a working reference (Rt) translated relative to the inertial reference ( Ri) and whose origin moves along a reference inertial trajectory whose carrier is assumed to be close (origin of the geographical reference point for the known positional ground alignment, GPS trajectory for the flight alignment, etc.). ). This simplified mode comprises the repetition of the following steps to estimate a state of a mobile carrier (P): - propagation (PROP) determining a current estimated state from a previous estimated state, measurements of the inertial sensors and the information a priori on the trajectory of the carrier (P) - update (MAJ) of the estimated state thanks to the information a priori on the trajectory of the carrier (P), Deterministic errors of the sensors (bias / drift / factors scale, etc.) are not estimated during the propagation and update stages. A disturbance step (PERT) then makes it possible to integrate the quantities ignored in the simplified mode and to estimate them by an optimization method.
    公开号:FR3013830A1
    申请号:FR1401512
    申请日:2014-07-04
    公开日:2015-05-29
    发明作者:Axel Barrau；Silvere Bonnabel
    申请人:Association pour la Recherche et le Developpement des Methodes et Processus Industriels；Sagem Defense Securite SA；
    IPC主号:
    专利说明:

    [0001] GENERAL FIELD The invention relates to the field of inertial navigation units, and carrier alignment methods comprising such plants.
    [0002] STATE OF THE ART Numerous solutions are known in the state of the art for aligning a carrier comprising an inertial unit. To achieve an alignment, we start from a priori on the approximate trajectory followed. For example, the trajectory followed by a stationary carrier at a known point on the Earth's surface is a zero motion if one places oneself in the geographical reference frame and a rotation movement around the axis of the poles if one place in an inertial reference system attached to the center of the Earth. This trajectory is never accurate (a car undergoes the vibrations of its engine, vertical movements when passengers enter and leave, etc.) but always close to reality. The classic alignment solution (also used by your new method) is to consider this trajectory a priori as a measurement from a virtual sensor. For example, for a stationary vehicle equipped with an inertial unit, it will be considered that there is a speed sensor always giving the "zero" response if one has placed in the geographical reference, and giving the speed of your trajectory a priori if one is placed in another reference. We can also consider that we have a position sensor always giving the "zero" response if we have placed in the geographical reference, and giving the position of the trajectory a priori if we have placed ourselves in a another landmark. You can also use both sensors at the same time (since they do not exist). We attribute to this virtual sensor a measurement noise which corresponds in reality to our own uncertainty on the trajectory. For example, always in the case of a stationary carrier, if one has chosen to align using a displacement information of zero, one will add a noise whose standard deviation is a few centimeters (the device which is Align can indeed deviate a few centimeters from the trajectory a priori, for example if you load and unload the trunk of a car). It is to integrate this (virtual) measurement that a Kalman filter usually intervenes. The implementation of such a Kalman filter typically comprises a step of propagation and a step of update repeated in time: the propagation step calculates an estimated state of the carrier and an uncertainty relating to this estimated state; the updating step makes it possible to integrate information a priori on the trajectory. However, these propagation and update steps involve nonlinear equations that prevent the use of a conventional Kalman filter. To circumvent such a limitation, it has been proposed to implement an "extended" Kalman filter (FKE), in which the matrices used in the non-linear equations corresponding to the propagation and update stages make the object of a tinearization, by a development at the order 1 at a given point of the vector space associated with the selected state vector. In particular, it has been proposed in EP 1 862 764 an alignment method using extended Kalman filters, based on the integration of a displacement information or a low speed information. During the implementation of this method, the trajectory of the carrier is calculated from inertial data and an estimation of its orientation, then the (virtual) observation of a zero speed or displacement ( to a sound close) is integrated by means of an extended Kalman filter. However, such an alignment method only works if the point in which the order 1 development performed to linearize the equations of the extended Kalman filter is sufficiently accurate; the estimation error induced by this development must indeed be low (a few degrees maximum). If the linearization point is not precise enough, the calculations performed by the extended Kalman filter during the propagation and update stages can eventually lead to biased estimates.
    [0003] Two preliminary steps have been proposed to determine a precise linearization point during the implementation of an extended Kalman filter applied to the alignment of a carrier: a vertical search, during which the vertical is estimated whereas the accelerometer observes gravity (approximately 2 seconds), a search for a coarse heading, during which course is estimated by a filtering method based on a simplified model (approximately 1 minute). These preliminary steps are not only long and complex to perform, but do not protect the inertial unit against future divergences (the filter estimate moves away from the actual state). PRESENTATION OF THE INVENTION The invention therefore aims to allow the alignment of a carrier in a more precise and faster way than with the techniques of the prior art mentioned. There is provided a method of aligning a moving carrier in an inertial frame and including an inertial unit, the inertial unit comprising a plurality of inertial sensors and at least one invariant Kalman filter, the method comprising repeating the following steps to estimate a state of the carrier representative of its speed and orientation: Propagation determining a current estimated state from a previous estimated state and measurements of the inertial sensors, 30 Update of the estimated state through the information a priori on the trajectory of the carrier, the inertial sensors (accelerometer and gyroscope or gyroscope for example) measure an acceleration and an angular velocity, both expressed in the reference of the central. These data are involved in the propagation step: the current estimated state (orientation, velocity and possibly position) is then obtained by integrating the angular velocities measured by your gyroscopes or gyrometers (updating of the orientation) and the measured accelerations. by your accelerometers (update of the speed). These accelerations are first projected in the work bench using the estimated orientation, and then we subtract the gravity because the accelerometer is not able to differentiate this force from an acceleration. If we want to estimate a position, we can integrate the speeds obtained, this operation also being part of the propagation. The updating step is the integration of the speed or position information given by the virtual sensor. This information (resulting from the a priori on the trajectory) is called - zero speed information "or" zero displacement information ". This step updates all data (orientation / speed / position) from the speed or position information. Indeed, the zero speed / displacement information (the) contains implicit information on the orientation. If it is initially assumed that the orientation is well estimated, the accelerometer measures in its reference a magnitude equal to the sum of the real acceleration and the gravity. As the control unit is stationary, the actual acceleration will be very low and the measurement will contain almost only gravity. During the propagation step, the measured acceleration is projected into the inertial coordinate system (or working reference) using the estimated orientation matrix. The quantity obtained is the gravity, this time expressed in the inertial reference (or reference work). We subtract your (known) gravity and get an acceleration very close to zero. This magnitude is integrated over time and we get a speed too very low.
    [0004] Assuming, however, that the estimated orientation is tilted a few degrees eastward from your reality, when your acceleration information is projected into the inertial frame using this false estimate of orientation, the vector obtained does not coincide with gravity. Once subtracted from (known) gravity, we obtain a non-zero vector, pointing to the west. After integration over time, we obtain a non-zero speed directed towards the west. Poor orientation therefore has an influence on the speed estimate. Conversely, a non-zero speed at the time of your update is interpreted as an orientation error. This is what a Kalman filter does (extended, invariant or otherwise). Thus, the update only requires the acquisition of "zero speed" or "zero displacement" (virtual) information, but no actual speed measurement or orientation. This (virtual) measurement is sufficient to find the orientation of the plant. According to one aspect of the proposed method, to allow a treatment with an invariant Kalman filter, a simplified mode is implemented in which. each speed occurring in the update equations is expressed in a work reference translated relative to the inertial reference frame and whose origin moves along a reference inertial trajectory whose carrier is assumed to be close, the sensor biases not being estimated during propagation and update steps. The origin may be that of the geographical reference (for ground alignment in known position, for example); it may correspond to the GPS trajectory (for flight alignment, for example). Advantageously also, a processing is used in parallel in which additional quantities influencing the estimate are estimated in the following manner: at each instant, the differential with respect to the additional quantities of the estimate provided by the simplified mode is calculated recursively. the additional quantities are calculated by optimizing a criterion of quality of the estimate. The differential calculus used makes it possible to express the estimated state of the wearer as a function that refines the additional quantities. In another aspect, there is also provided a mobile inertial unit in an inertial frame and comprising: a plurality of inertial sensors, an invariant Kalman filter, said central comprising an onboard computer implementing the proposed alignment method. DESCRIPTION OF THE FIGURES Other features, objects and advantages of the invention will emerge from the description which follows, which is purely illustrative and nonlimiting, and which should be read with reference to the appended drawings, in which: FIG. inertial device according to one embodiment of the invention. Figure 2 shows an inertial unit according to one embodiment of the invention. Figs. 3A and 3B are schematic diagrams of an extended Kalman filter and an invariant Kalman filter, respectively. Fig. 4 shows the steps of a method of aligning an inertial unit, according to an embodiment of the invention. FIGS. 5A and 5B are cap alignment curves obtained by implementing methods according to embodiments of the invention, and by implementing a known method. FIGS. 6A and 6B are comparative curves of heading error obtained by implementing the method of the invention, FIG. 6B being a close-up view of 6A.
    [0005] FIG. 7 illustrates, for the sake of comparison, the progress in time on the one hand a method implementing a simplified alignment mode and a complete mode and on the other hand a method of conventional type.
    [0006] In all the figures, similar elements bear identical references. DETAILED DESCRIPTION OF THE INVENTION Referring to FIG. 1, a carrier P is mobile in a predetermined inertial reference frame defined by an inertial reference frame Ri. An example of an inertial landmark is the landmark centered on the center of the Earth, whose z axis points to the North Pole, whose x axis points to the intersection of the Greenwich meridian and the equator at time t = 0 (the point thus defined will then move in our landmark because of the rotation of the Earth) and whose y axis points in the direction of the vector zxx, where x denotes the vector product). The carrier mark Rp is fixed relative to the central. For example, the axes used most frequently are directed to the front, right and bottom of the unit. Its origin is a fixed point of your plant. The geographical reference point Rg originates from the position of the power station and is composed of three axes directed towards the north, the west and the top. Also shown is a reference mark Rt, which is the "working reference mark", which is the reference in translation with respect to the inertial reference mark Ri (that is to say, that of axes aligned with the axes of the inertial reference mark Ri) of which origin C is that of the geographical reference Rg. With reference to FIG. 2, the carrier P comprises an inertial unit IN. The inertial unit IN comprises a plurality of inertial sensors 30 CV, CO and a computer configured to implement an invariant Kalman filter K.
    [0007] The inertial unit IN is assumed to be fixed with respect to the working reference mark Rt. The plurality of sensors comprises at least one acceleration sensor CV (for example, an accelerometer) and at least one angular velocity sensor CO (for example, one gyroscope or a gyrometer). The invariant Kalman filter K is a recursive estimator of a state of the carrier, this state being representative of the speed of this carrier and its orientation. The invariant Kalman filter K is more particularly configured to determine a current estimated state of the carrier from a previous estimated state of the carrier by means of predefined propagation equations: this is called propagation. The propagation equations are predetermined from the measurements of the inertial sensors. The invariant Kalman filter K is also configured to update the current estimated state by means of predefined updating equations, in which the prior information on the trajectory (for example the low speed information) is involved. . The invariant Kalman filter K is further configured so that each velocity involved in the update equation is expressed in the work reference. The central IN can for example build its work bench by means of GNSS / GPS receivers. The step which will now be described is repeated from the first moment of the alignment procedure. The first estimate is unspecified. It is assumed that a previous estimate of the state x, denoted 2t_1, has already been determined, as well as a matrix P representing the uncertainty of the estimate. With reference to FIG. 3A, the determination of a current estimate δi + by a known extended Kalman filter depends on the previous estimate z (with a propagation step) and measurements acquired during the current iteration ( by means of linearization and updating steps) The calculation of 2+ involves a gain matrix L itself dependent on the value of 2. The introduction of the work reference point Rt and a simplified model ignoring the sensor bias allows here the use of an invariant filter. Referring to FIG. 3B, an invariant Kalman filter calculates the current estimate 2+ of the state differently from an ordinary extended Kalman filter, using specially selected equations (these equations will be detailed further). The state of the carrier can be represented either conventionally by a speed v and an orientation T (T is a rotation matrix or a quaternion), or by a displacement matrix x of the form: X = (T vl 15 D other representations are of course possible.It should be noted here that the name of "matrix of displacement" does not necessarily represent a displacement.It comes from the use that was done when they were introduced for robotics problems (they contained then an orientation and a position and represented a displacement.) The uncertainty of estimation of the state x is called P. An interesting property of the invariant filter K is that the gain matrix L intervening in the putting step As a result, a simplified system for the use of an invariant Kalman filter (and therefore a convergence without steps of vertical search and alignment) has been constructed. rossier), having the following characteristics: 1) We work in an unusual frame. 2) The deterministic part of the errors of the inertial sensors (bias / drifts / scale factor errors) are ignored. 3) Gravity is considered to depend only on time and not on position. 4) The prior information no longer relates to the speed or displacement in the chosen repository (the so-called work repository), but on their projection in the repository of the carrier. The conventional fine-alignment method does not go through a simplified model. It directly integrates all the variables in the model, which does not allow the use of an IEKF. The step of updating an IEKF for the alignment is written either with displacement matrices, or with the initial variables v and T of the work referential (by extracting displacement matrices): 10 - Vs + = 1 (, (e) + me »- = K - K and Ki, are functions coming from the map 15 - P follows a time-dependent equation, which is obtained by linearizing the equations verified by 8v = 19 -1-7-iv and ST = r'T-1 around 0 and / 3 (the identity matrix of size 3 x 3) The exponential map exp is defined for u E IE86 by exp (u) = 20 exp ( D (u)) where expm denotes the classical matrix exponential and where D () is the linear map taking as argument a vector x = (xl) of size 6 x 1 and making the elementary displacement matrix (A (x x2) of size 4 x 4, where A (x1) denotes the antisymmetric matrix 0 1 such that for any vector u E I183, A (x1) u = x1 xu, x being the vector product. u) having a particular form we can use the following closed formula: (ui) ± [1_cosiu, i] D (liq 2 + [11/21-sin 1u21] D (u1 3 exp GL2) = 14 D u2 1u212 u2) 1u213 W2) exponential defined below, such that exp (Ltx) = (Mx) Kv (x)) where Lt is a matrix dependent on P. 01x3 1 In one embodiment, the calculation just described is iterated up to 'to convergence (simplified mode). As a variant (complete mode), it is also possible to estimate additional quantities (bias, drifts, scale factor errors, etc.) by the method described below. Once this reduced filter is constructed, from the first moment of the alignment procedure, the differential of the estimated trajectory with respect to the remaining parameters (bias / drift / scale factor error) is calculated around the zero value of these parameters. settings. The estimated trajectory is then expressed as an affine function of the parameters. A quadratic quality criterion of the estimated trajectory (for example, the sum of the squares of the velocity norms estimated over time) is chosen. This criterion is then a quadratic function of the parameters (bias / drift / scale factor error), which is easily optimized to obtain their value. This method makes it possible to preserve the convergence properties of the filter constructed for the simplified system even if the number of estimated parameters is increased. This optimization can be done recursively over time, once at the end of the alignment, or at any time during the alignment. This choice has no influence on the estimate. In all your cases, the filtering of the simplified system and the complete system are simultaneous. In the methods employing a coarse alignment, this is also done on a simplified system (different from the one presented here) but the slicing is temporal: 1 min of coarse alignment then a fine alignment. Here the two models work simultaneously from the beginning to the end: at each instant the simplified filter gives its estimate, then the quadratic optimization calculates the complete state (without Kalman filtering) as a disturbance of the simplified state. The calculation of the trajectory differential is feasible in a reasonable time because your matrix P follows a time dependent equation (IEKF). If the simplified system was not an IEKF, this last step of estimating parameters ignored at first would not be possible.
    [0008] INERTIAL PLATFORM ALIGNMENT METHOD A method for aligning the INERENTAL IN (and therefore the P) carrier implementing the invariant Kalman filter K will now be described with reference to FIG. ACQ, your trajectory of the workmark Rt is updated thanks to the information a priori concerning your trajectory. In a propagation step PROP, the Kalman filter K determines a current estimated state from a previous estimated state, by means of the measurements of its inertial sensors CO, CV. In a update update step, the Kalman K filter updates the carrier's estimated state by means of virtual observation of zero velocity (or zero displacement). The steps of ACQU acquisition, PROP propagation, and update update are repeated in time. The alignment method therefore comprises several successive iterations, each new iteration taking as input the updated estimated state obtained during a previous iteration, and new observations (acquired during the acquisition step of this new iteration). A computer program product comprising program code instructions for executing the steps of the described method, when this program is executed by an on-board computer in an inertial unit, may furthermore be implemented.
    [0009] General formalization of the alignment problem As a preliminary, the following notations will be used in the following. i Matrix for the passage of the measurement reference to the inertial reference 77. T [1 ri Carrier speed in the inertial reference system xi Position of the carrier in the inertial coordinate system c Measurements of the gyroscope or gyrometer f Measurements of the accelerometer G Land attraction undergone by the carrier w ,, Noise of the gyroscope or the gyrometer ("random watt <" in English) wf Difference between the measured speed increment and the real speed increment The classical formulation of zero speed observation alignment in an inertial reference is the following. It is assumed that the position of the carrier is approximately known. The consequence of this hypothesis, which the process takes advantage of, is that local gravity is known and will be no more than a function of time. The equations of motion are written: = T ulA (co + w, ') vi = Tref + wf) + G xi = vi Where A (.) Designates the antisymmetric matrix associated with a vector of rotation (defined by the relation d u1, u2 E R3, A (u1) u2 = u1 x u2 where x is the vector product), A virtual measure of velocity (wh is a noise) is: h (T, vi) = + Wh will be introduced at intervals the information h (T [ml, rird1, vi) + wh = Vi, where Vi is the expected velocity of the carrier P related to the trajectory a priori, for example to that due to the terrestrial rotation (it is simply expressed in the inertial reference frame the observation of zero speed usually made in the geographical reference). Reformulation of the equations of motion in the work reference In this part are detailed the equations of propagation and updates implemented in one embodiment of the invention. The working reference mark Rt (also referenced C in the following) is as previously defined in translation with respect to the inertial reference frame Ri (that is to say that its axes are aligned with the axes of the inertial reference frame Ri), and its origin C is that of the geographical reference g. The following notations are introduced: T Matrix for passing the measurement mark towards the C mark FciTti xc = xi - C Position of the wearer in the C mark vc = ic = vi - ù = vi - yi Carrier speed in the C mark a = C Acceleration in the inertial reference frame of the origin c of the C mark The equations of motion of the carrier P are rewritten in the work coordinate reference Rt as follows: T [c1 = rne ,,, - [c: i (f wf The position xc of the carrier has no influence on the other 20 parameters, it can therefore be deleted: vc = T [cif + G a The zero velocity information is also modified as follows : h (T cc t & v = TTic v = 0 + noise the state of the carrier P estimated by the invariant Kalman filter K can be represented by a matrix of displacement constituted from a matrix of passage of the geographical reference to reference, and the measured carrier speed expressed in the work reference, the displacement matrix and constructed as follows: x (Tim V 0 1) Of course, alternatively, the state of the carrier could also be represented in a conventional manner, for example a matrix T and a vector v.
    [0010] Inertial measurements and gravity information are written in elementary displacement matrices: I (A (co) 0 Of w (il (w ') 0 0 = (0 G - O 0 The equations are then rewritten more simply: X = X.i + eX + Xw The observation function can also be written simply: h (x) = x-1 (1) h A property of h called - left-right equivariance - on which the invariant filtering is based The following is 0. Any displacement matrix (that is, of the form (Ro ui) where R is an orthogonal (3 x 3) matrix and u is any vector of). xO) = (x0) -1 rih) = 01x-1 Ceih) = 011 / (X) We can interpret the above as a change of variable on the state space (we replace x by x0) associated with a variable change in the observation space The term "invariant" comes from the fact that the filter is designed to be insensitive to this type of reparameterization.
    [0011] An observer adapted to the above dynamic is following you: The updating step MAJ is typically of the following form: r = KtUM where i is the current estimated displacement matrix, Kn, a gain function (also called gain ), Y is a vector representing the virtual observation of zero velocity, and 2+ is an estimated displacement matrix resulting from the update. The function Kt corresponds to your t-th observation of zero velocity (and therefore the ith iteration of the Kalman filter) and is of the form Kt (u) = exp (Ltu), where exp () denotes your function called map 15 exponential map (in English). Kt is a function but Lt is a matrix, called "gain matrix" or more simply - gain. The method of calculating earnings is detailed below. The matrices Lt are calculated by linearizing the equation verified by the error variable = 2x-1. The computation of the equation verified by ri is as follows: During the propagation step, the evolution equations of x and l are known and we can therefore calculate the evolution equation of n: = (dtk X-1 + X (- dt X-1) = dt X-1 _ x-1 (d dtx) X-1 By introducing the equations checked by i and x and using the fact that vc is small, we obtain: = .917 -119 -71w This is the continuous part of the evolution of the error variable. When the estimate is updated, the error variable changes discontinuously. The evolution of the error variable tors of the update of the estimate is particularly simple: n + = rx-1 = Kt (e) x-1 = Kt (h (.lx-1, wh)) fcx -1 = Kt (h (n, wh)) n The linearization of this equation is as follows: The exponential map takes as argument a vector of R6 and makes a displacement matrix. In particular, the image of the null vector is the identity matrix 14. We can therefore make the error variables correspond to the vectors of e and (77 = expw, 77+ = exp (Ç +)), write the verified equation by e and CF and tinearize around 0. The equation obtained involves the 10 matrices of gains Lt: + (wwfl = - Lt (He + w) with adgt = (Acm, 03 03 03), 03 being the matrix 3 x 3 containing only zeros, and H = This equation is linear and it is possible to calculate simultaneously the covariance Pt of and the gains Lt making it possible to minimize Pt by means of the Riccati equation: Pt = aclgtPt + Pt acrl: qt + Q St = HPOT + R Lt = POT St-1 Pt + t = (16-LtH) Pt where Q is your noise covariance matrix (wwfl and R the noise covariance matrix wh 20 Comparative alignment results 13 (03) The following are compared results obtained with the alignment method just described (based on an invariant Kalman filter) and the results obtained with a known alignment method (based on a classical extended Kalman filter and implemented in a conventional power plant). The variables processed by your Kalman filters are the attitude (attitude) of the wearer and his speed. The position of the carrier is considered known to a hundred meters. The sensors used are assumed to be unbiased, and the observations used are zero speeds. The vertical of the carrier is known to 30 degrees, and his heading completely unknown. Figs. 5A and 5B each compare the respective convergence rates of the two alignment methods, and show course estimates provided by both methods. In the example illustrated in FIG. 5A, the integration of the zero velocity information is done at low frequency. It is observed that after 200 seconds, the alignment method according to the invention (blue curve) provides a heading estimate very close to its real value, whereas the heading estimate provided by the conventional method (curve green) continues to oscillate. In your situation studied, your earnings used in the update step can be calculated in advance if you know the latitude. The filter is then very inexpensive in computing time and one can consider integrating the high speed zero velocity information as shown in FIG. 5B. The following figure shows the consequences of this choice in the same situation as before. Convergence here is much faster with the proposed method than with the method of the prior art. Figures 6A and 6B study the influence of a prior estimate of heading on the results of the alignment method of the invention.
    [0012] Course estimates were randomly drawn according to a uniform law on [0.27r] and then the algorithm worked normally for 240 seconds. It can be observed in FIG. 6A that the heading error quickly becomes close to zero whatever the initialization during the implementation of the method according to the invention. Figure 6B is a zoom of Figure 6A. Complete mode implemented in parallel with the simplified mode The simplified mode just described is advantageously completed by a complete mode which is implemented in parallel with the simplified mode. This is illustrated in FIG. 7, on which is represented the time course of one part of a method implementing a simplified alignment mode and a complete mode and on the other hand a classic type method This complete mode allows quadratic optimization alignment. It makes it possible to estimate additional quantities influencing the estimate in the following way: at each instant, the differential with respect to the additional quantities of the estimate provided by the simplified mode is calculated recursively. the additional quantities are calculated by optimizing a quality criterion of the estimate. The calculation of the additional quantities by optimization of a quality criterion can be implemented at the end of said alignment method. It can also be implemented permanently or in a simplified manner, at any time of the alignment process.
    [0013] The differential calculus implemented makes it possible to express the estimated state of the wearer as an affine function of the additional quantities. By comparison, the conventional method implements successively: A vertical search processing; A rough estimate of the state of the system (coarse alignment); Alignment of the complete system by integrating virtual observations of displacement and / or zero velocity by extended Kalman filtering). In the case proposed, an example of implementation of a complete mode by quadratic optimization is the following: If the measurements of the inertial sensors undergo a deterministic perturbation linked to additional quantities (bias, drifts, errors of scale factors , etc.), represented by a vector that must be evaluated during the alignment, these are not integrated in the Kalman filter as in conventional methods. The following procedure is used: For any given value of fl we can correct the inertial measurements (subtract the drift-bias given by beta, divide by the scale factor, etc.) and then apply the simplified mode to the corrected measures. The estimate obtained is a function (e) of the additional quantities. This quantity is not calculated because we do not know 25. We choose a reference value p'0 of your magnitude, the only estimate 2 (f ) Will actually be calculated. Any initialization of the magnitudes involved in the simplified mode is chosen. We choose a likelihood function that applies to a trajectory estimation: f (2 (/ 3), x1 (/ 3),, t (f3)) - The following steps are repeated over time: - Step Propagation (PROP) Inertial measurements are used to compute the propagation step of Xt ((30) by the simplified model, as well as the Pt matrix.
    [0014] The equation verified by 2t (p) is linearized around po to recursively evolve the affine application Axt: f3 -> 50), which is the first-order expansion of Xt ((3) around f30. for example represented by a matrix - Update step (MAJ): The prior information on the trajectory is used to update 5zt ([3,3] by the method described in the simplified model. Xe update) is the same as 2t (f30) (this is a specificity of the invariant Kalman filter), it is used to update its first order Axt development. - Disturbance Phase (PERT): This step can be applied once at the end of the alignment, at different times during the alignment or at each time step. An estimate p of ig is obtained by optimization: = argminp f (2.0 (f3), Mig), ..., W)). A refined estimate x of the state x is then given by It = Axt (P) or another function of p, equivalent to Axt in the first order. The function f can be quadratic. It is then not necessary to store all values of 200) or Axis. The function f can involve additional quantities calculated recursively over time. The function f can for example be defined by: ah ah f (5c-21 (16), tt (13)) = [IM] T i st1 [MM] at (f30) ait (o) i = 0 Where St is given by the simplified mode and the symbol T denotes the matrix transposition. Another example of choice of function f, used in the following, is: 2 f (10 (l61),, £ te) ah lax -, (6,0) Pt Ce)] The computation of / can be done only one once at the end of the alignment, at different times during alignment, or continuously during alignment. A recursive optimization method can be used to compute p.
    [0015] The estimate p can possibly be used as a new reference value for the continuation of the alignment by setting po = The refined estimate ite can possibly be used as the last estimate in the simplified model for the continuation of the alignment by posing 2t (i3o) = £ t (p). An example of implementation of the method is described below: 13. (fli) is a vector of size 6 × 1 containing the drifts of λ2 / three gyroscopes or gyrometers ([31] and the biases of the three accelerometers 20 ( / 32). Simplified mode variables are orientation and speed. They (T c, can be written in a matrix of displacementxt = [7i, vc 0 1 One chooses flo = 0 as reference value of the biases and drifts The measurements of the inertial sensors are thus used without modification 25 in the simplified mode .
    [0016] If we take into account the biases and drifts denoted by [3, the estimate Tc (l3) oce) is (P) =, of which only a first order approximation 0 1 around a reference value Ai will be calculated, follows the The following equation of propagation: d dct6 (3) = lt (re). (i- + 2t (f3) And the following update equation: Ite + = exp (-Ltete) 2t where expQ is the exponential map taking In argument a vector of size 6 x 1 and making a matrix of displacement yarn is the following elementary displacement matrix: p (ceil) fl12) The gain matrix Lt is given by the simplified mode. A simple way to write the first-order estimate of 2t (3) is: jal) = fct (0) + tt (0) D (Ztfl) where D () is the linear mapping of a vector x = (x2) of size 6 x 1 and making the elementary displacement matrix (44 (x01) x2) of size 4 x 4. The matrix Zt is of size 6 x 6 and satisfies 1 the following propagation equation: d dt Zt = adiZt - 16 And the following update equation: Zt = 16 -xt Dexp (-LtV (5 (-3) 14 (0 i'iâ) Zt 20 Where the matrix Lt is given by the simplified mode , the displacement matrices jit and 2: - are given by the simplified mode, the rotation matrix i "rci is given by the simplified mode, J (A (cot) 0 the matrix adj is defined by adj = (col and ft are the A (ft) A (0.) t) measurements of the inertial sensors), the matrix Ad + - ~ is defined by Ad + - ~ = Xt Xt A (- 't) [±; ciii) T, the function V takes as argument a displacement matrix of size 4 x 4 (here the matrix Xt = D [m1vt) and 0 1 makes the vector of size 3 x 1 located at the top right (here iit). The function Dexp is the derivative of the exponential map in the following sense: exp (i; + u) = [14 + D (Dexp () u)] exp (0 where and u are vectors of size 3, u is standard much less than 1, 14 is the identity matrix of size 4 and Dexp () is a matrix of size 6 x 6.
    [0017] The chosen likelihood function is defined by the following formula: f (£ .e, £ .1 (6 '), £ t (ie)) = It is rewritten: ah [tt (P)] i lax-r (flo ) ti = 0 2 2 f (£ 0 (P), ii (P),, £ "t (ig)) i = 0 -1" Tc [A (rTc ec), I3] Zag [771 No therefore, it is not necessary to store information on the whole trajectory, it suffices to write f (e) = f3TS113 + S2 p + constant and to maintain the matrices S1 and S2 by developing the expression of f. The complete algorithm (simplified mode and complete mode) is therefore as follows: The reference value of the biases and drifts is / 30 = O. The initial estimate 20 (0) and the covariance matrix P (representing the uncertainty) are initialized in any way that may depend on the situation. The matrix Zt representing the first order development of Xt W) is initialized to Z0 = 06x6. The matrices S1 and S2 are initialized at S1 = 06x6 and S2 = 6X1. Then the following steps are iterated for the duration of the alignment: - Propagation stage (PROP): Propagation of the uncertainty: i't = acigtPt + Pt ad7g.t + Q Propagation of the estimate: d dtMO = "it (0) -i + g t.
    [0018] Propagation of first order development: d = dt acitZt - - Update stage (MAJ) at times tt: Update of uncertainty: St = H POT + R Lt = Ptj-IT St-1 Pt +, = (I6 ktH) Ptt Update estimate: f (iF (0) = exp (Ltetf (0)) E0) Update first-order development: 11-1! = 16 - xt Dexp (-Lt12Ci t)) Lt (0 1-iTM) Zt T = S1 + ([11 (liTrin (0) T c (0)), 13Z t) ([4 (licrniT c (0) ), 13 S2 S2 + (i'Zi (0) T i3 c. (0)), 31Z t) DETcnes c (0) 20 zt) - Disturbance stage (PERT): Estimation of biases and drifts: e is the solution of the linear system Si; 6 '= refined estimation of the quantities of the simplified mode: it (r3) = 5- (texpczt (3) The function exp can be replaced by another function equivalent to the first order. Other modes of implementation To improve the robustness and the performances of the method, it is possible to use several times in parallel the method described, initialized on different orientations and then to select one or more of the estimates obtained.Various selection criteria are possible. Calculation related to the propagation step, one can solve the equations verified by Xt and Zt in a generic form of the type: d Gt, = 14 dT Gt -> T = gt- Gt - a (1) d = = Jt, T, i (2) d At-n-14 dT t + T adi (3) d Bt-n = 0 dT Bt-> T (4) 2T = Gt-41 'ft -> T (5) ZT = 14t, 7. (Zt B t_> T) (6) Equations (1), (2), (3) and (4) are computed only once in the propagation step, equations (5) and (6) applied the quantities 2 and Z associated with each of the filters operating in parallel, at the beginning of the updating step. You can also use your same earnings for all filters.
    [0019] The selection of the estimate is done by calculating a likelihood criterion for each of the filters and then selecting the best result. This criterion can be for example a quadratic function of speed estimates over time. Also, several estimates can be retained on a coherence criterion, for example the two (or more) giving the closest results. One of them or any function of these estimates, for example an average, is then chosen as the final estimate.
    [0020] Moreover, all the described methods can be realized by replacing the virtual observation of speed by a virtual observation of position. The simplified state then also contains the position and is placed in a matrix of the form: T [cym] vc xc ° 1X3 ° 1X3 1 0 0 1 X = The observation function becomes: Wh h (x) = x- 1 (0) 1 The rest of the calculations are conducted in the same way as for a zero speed observation. Alternatively again, all of the described methods can be performed by representing the data in some other way. For example, one could parameterize the orientations by quaternions or Euler angles and transpose all the calculations. Also, the Kn (.) Gain functions can be chosen by any other method than the one proposed. For example, a Unscented Kalman Filter procedure can be applied to the nonlinear error equation, using constant gains optimized on simulations, and so on.
    [0021] In addition, any initialization method may be added at the beginning of the alignment procedure. For example, accelerometers can be used to search for vertical.
    [0022] The methods described can be integrated into a larger architecture, such as federated extended Kalman filters, a Kalman filter bank, and so on.
    [0023] On the other hand, if the inertial unit is not placed at the same place as the point on which the position information is concerned, but this point is fixed and known in the reference frame of the central unit (for example if the GPS receiver is far from the central unit), the same procedure can be applied as for the alignment by virtual observation of zero displacement, by replacing zero by your known position (the lever arm separating the two receivers in the case of a GPS and a central ).
    权利要求:
    Claims (10)
    [0001]
    REVENDICATIONS1. A method of aligning a moving carrier (P) in an inertial frame (Ri) and including an inertial unit (IN), the inertial unit comprising a plurality of inertial sensors (RV, RO) and at least one invariant Kalman filter , the method comprising the repetition of the following steps for estimating a state of the carrier (P) representative of its speed and its orientation: - Propagation (PROP) determining a current estimated state from a previous estimated state and measurements of the sensors inertial, - Update (MAJ) of the estimated state by the prior information on the trajectory of the carrier (P), the method being characterized in that, to allow treatment with at least one invariant Kalman filter , a simplified mode is implemented in which each speed involved in the updating equations is expressed in a working reference (Rt) translated relative to the inertial reference (Ri) and whose origin moves the lo ng of a reference inertial trajectory whose carrier is assumed to be close, the sensor biases are not estimated during propagation and update steps.
    [0002]
    2. Method according to claim 1, characterized in that in the simplified mode, each position involved in the updating equations is expressed in a working reference (Rt) translated relative to the inertial reference (Ri).
    [0003]
    3. Method according to claim 1, characterized in that a process is also implemented in parallel in which additional quantities influencing the estimate are estimated in the following manner: at each instant, the differential with respect to Additional quantities of the estimate provided by the simplified mode is calculated recursively. your additional quantities are calculated by optimizing a quality criterion of the estimate.
    [0004]
    4. The method of claim 3, wherein the calculation of the additional quantities by optimizing a quality criterion is implemented at the end of said alignment process. 10
    [0005]
    5. Method according to claim 3, wherein the calculation of the additional quantities by optimization of a quality criterion is implemented continuously or at one or more times of the alignment method. 15
    [0006]
    6. alignment method according to one of the preceding claims, wherein a series of gains is calculated before the repeated implementation of the simplified mode propagation and update (MAJ) steps, each gain occurring in a iteration of the update step. 20
    [0007]
    7. Method according to one of the preceding claims, characterized in that the work reference (Rt) originates from the origin of the geographical reference.
    [0008]
    8. Method according to one of claims 1 to 6, characterized in that the origin of the work reference follows a GPS trajectory.
    [0009]
    9. Inertial unit (IN) movable in an inertial frame (Ri) and comprising: a plurality of inertial sensors (CV, CO), an invariant Kalman filter, an onboard computer of said plant implementing the repetition of following steps to estimate a state of the carrier (P) representative of its speed and orientation: - Propagation (PROP) determining a current estimated state from a previous estimated state and measurements of the inertial sensors, - Update ( MM) of the estimated state thanks to the information a priori on the trajectory of the carrier (P), characterized in that, to allow a treatment by an invariant Kalman filter, the computer is configured to implement a method alignment device according to one of claims 1 to 8.
    [0010]
    10. Computer program product characterized in that it comprises program code instructions for the execution of the steps of the method according to one of claims 1 to 8, when this program is executed by a computer embedded in a central inertial, can also be implemented.
    类似技术:
    公开号 | 公开日 | 专利标题
    EP3071934B1|2018-01-03|Alignment procedure of an inertial measuring unit
    EP2428934B1|2021-07-14|Method for estimating the movement of a carrier in relation to an environment and calculation device for a navigation system
    EP1886517B1|2010-05-19|Method and device for locating a terminal in a wireless local area network
    EP3213033B1|2018-08-22|Method of estimating a navigation state constrained in terms of observability
    Kneip et al.2011|Deterministic initialization of metric state estimation filters for loosely-coupled monocular vision-inertial systems
    EP2718670B1|2020-12-23|Simplified method for estimating the orientation of an object, and attitude sensor implementing such a method
    WO2014076294A1|2014-05-22|Method for determining, in a fixed 3d frame of reference, the location of a moving craft, and associated computer program and device
    FR2878954A1|2006-06-09|HYBRID INERTIAL NAVIGATION SYSTEM BASED ON A CINEMATIC MODEL
    WO2016156602A1|2016-10-06|Method for tracking the navigation of a mobile carrier with an extended kalman filter
    EP2241856A1|2010-10-20|Hybrid inertial system with non-linear behaviour and associated hybridising method using multi-hypothesis filtering
    EP3060881A1|2016-08-31|Method for indoor and outdoor positioning and portable device implementing such a method
    EP3447654A1|2019-02-27|Method for determining the trajectory of a moving object, program and device for implementing said method
    FR3042266A1|2017-04-14|METHOD FOR ESTIMATING THE MOVEMENT OF A PIETON
    CN107884800B|2020-06-26|Combined navigation data resolving method and device for observation time-lag system and navigation equipment
    FR3064350A1|2018-09-28|METHOD FOR CALCULATING A SPEED OF AN AIRCRAFT, METHOD FOR CALCULATING A PROTECTIVE RADIUS, POSITIONING SYSTEM AND ASSOCIATED AIRCRAFT
    EP3403054B1|2020-03-18|Device and method for maintaining the attitude of a carrier using gyroscopes
    FR3069316B1|2019-08-16|METHOD FOR ESTIMATING THE MOVEMENT OF A EVOLVING OBJECT IN A MAGNETIC FIELD
    Radi et al.2020|Accurate identification and implementation of complicated stochastic error models for low-cost MEMS inertial sensors
    WO2016087784A1|2016-06-09|Method of estimating the motion of a carrier with respect to an environment and calculation device for navigation system
    EP3623758B1|2021-04-21|Positioning system, and associated method for positioning
    FR3097316A1|2020-12-18|Method for monitoring the performance of inertial measurement units
    EP3827220A1|2021-06-02|Method and device for assisting with the navigation of a fleet of vehicles using an invariant kalman filter
    EP3827221A1|2021-06-02|Vehicle navigation assistance method and device using an invariant kalman filter and a navigation status of a second vehicle
    EP3211370A1|2017-08-30|Method for filtering signals from a sensor assembly comprising at least one sensor for measuring a vector physical field that is substantially constant in time and space in a frame of reference
    WO2015086587A1|2015-06-18|Method and device for aligning an inertial unit
    同族专利:
    公开号 | 公开日
    CN106068441B|2019-03-22|
    WO2015075248A1|2015-05-28|
    FR3013829B1|2016-01-08|
    EP3071934B1|2018-01-03|
    EP3071934A1|2016-09-28|
    CN106068441A|2016-11-02|
    FR3013829A1|2015-05-29|
    US10859380B2|2020-12-08|
    FR3013830B1|2016-01-08|
    US20160290808A1|2016-10-06|
    引用文献:
    公开号 | 申请日 | 公开日 | 申请人 | 专利标题
    US20040150557A1|2003-01-21|2004-08-05|Ford Thomas John|Inertial GPS navigation system with modified kalman filter|US10345427B2|2015-04-01|2019-07-09|Safran Electronics & Defense|Method for tracking the navigation of a mobile carrier with an extended kalman filter|US5610815A|1989-12-11|1997-03-11|Caterpillar Inc.|Integrated vehicle positioning and navigation system, apparatus and method|
    US5438517A|1990-02-05|1995-08-01|Caterpillar Inc.|Vehicle position determination system and method|
    US7209810B2|2002-01-10|2007-04-24|Lockheed Martin Corp.|Locomotive location system and method|
    US6697736B2|2002-02-06|2004-02-24|American Gnc Corporation|Positioning and navigation method and system thereof|
    US7512493B2|2006-05-31|2009-03-31|Honeywell International Inc.|High speed gyrocompass alignment via multiple Kalman filter based hypothesis testing|
    FR2904870B1|2006-08-09|2008-10-03|Sagem Defense Securite|METHOD FOR ALIGNING AN INERTIAL PLANT WITH AXISYMETRIC VIBRANT SENSOR AND CORRESPONDING INERTIAL PLANT|
    FR2922300B1|2007-10-12|2009-11-20|Saint Louis Inst|METHOD FOR DETERMINING THE ATTITUDE, POSITION AND SPEED OF A MOBILE MACHINE|
    US20110238308A1|2010-03-26|2011-09-29|Isaac Thomas Miller|Pedal navigation using leo signals and body-mounted sensors|
    CA2848217C|2011-09-14|2018-09-18|Trusted Positioning Inc.|Method and apparatus for navigation with nonlinear models|US10088318B2|2015-08-27|2018-10-02|Qualcomm Incorporated|Cradle rotation insensitive inertial navigation|
    US10386203B1|2015-11-05|2019-08-20|Invensense, Inc.|Systems and methods for gyroscope calibration|
    FR3043469B1|2015-11-10|2019-10-18|Safran Electronics & Defense|METHOD FOR DETECTING PARASITE MOVEMENTS DURING STATIC ALIGNMENT OF AN INERTIAL POWER PLANT, AND DETECTION DEVICE THEREOF|
    CN108759870A|2018-07-03|2018-11-06|哈尔滨工业大学|A kind of Transfer Alignment based on New Type of Robust General High-order volume Kalman filtering|
    CN109189087B|2018-08-20|2020-07-14|哈尔滨工业大学|Self-adaptive fault-tolerant control method for vertical take-off and landing reusable carrier|
    法律状态:
    2015-06-25| PLFP| Fee payment|Year of fee payment: 2 |
    2016-06-22| PLFP| Fee payment|Year of fee payment: 3 |
    2017-01-13| CD| Change of name or company name|Owner name: ASSOCIATION POUR LA RECHERCHE DEVELOPPEMENT DE, FR Effective date: 20161214 Owner name: SAGEM DEFENSE SECURITE, FR Effective date: 20161214 |
    2017-01-13| CJ| Change in legal form|Effective date: 20161214 |
    2017-06-21| PLFP| Fee payment|Year of fee payment: 4 |
    2018-06-21| PLFP| Fee payment|Year of fee payment: 5 |
    2020-06-23| PLFP| Fee payment|Year of fee payment: 7 |
    优先权:
    申请号 | 申请日 | 专利标题
    FR1302705A|FR3013829B1|2013-11-22|2013-11-22|METHOD FOR ALIGNING AN INERTIAL PLANT|
    FR1401512A|FR3013830B1|2013-11-22|2014-07-04|METHOD FOR ALIGNING AN INERTIAL PLANT|FR1401512A| FR3013830B1|2013-11-22|2014-07-04|METHOD FOR ALIGNING AN INERTIAL PLANT|
    US15/037,653| US10859380B2|2013-11-22|2014-11-24|Alignment method for an inertial unit|
    EP14803092.7A| EP3071934B1|2013-11-22|2014-11-24|Alignment procedure of an inertial measuring unit|
    PCT/EP2014/075439| WO2015075248A1|2013-11-22|2014-11-24|Alignment method for an inertial unit|
    CN201480064042.XA| CN106068441B|2013-11-22|2014-11-24|The calibration method of inertance element|
    [返回顶部]